
#include "rviz_custom_plugins/utils/interactive_marker_factory.h"
#include <pcl/segmentation/sac_segmentation.h>
#include "rviz_custom_plugins/utils/interactive_marker_util.h"
#include <pcl/filters/extract_indices.h>
#include <rviz_common/logging.hpp>
#include "rviz_custom_plugins/utils/pcl_util.h"
#include <pcl/features/moment_of_inertia_estimation.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>


namespace rviz_custom_plugins
{

namespace utils
{


int InteractiveMarkerFactory::incrMarkerId = 0;


std::string InteractiveMarkerFactory::generateIntMarkerName(std::string markerType)
{
    std::string markerName;
    markerName.reserve(20); // 预估总长度
    markerName.append(markerType).append("_").append(std::to_string(++incrMarkerId));
    return markerName;
}


InteractiveMarker InteractiveMarkerFactory::createWaypoint(Pose pose)
{
    visualization_msgs::msg::Marker marker;
    marker.type = visualization_msgs::msg::Marker::ARROW;
    marker.scale.x = 0.5; // 尺寸
    marker.scale.y = 0.1;
    marker.scale.z = 0.1;
    marker.color.r = 0.0; // 颜色（RGBA）
    marker.color.g = 1.0;
    marker.color.b = 0.5;
    marker.color.a = 1.0;

    // 添加移动控制
    visualization_msgs::msg::InteractiveMarkerControl movePlaneControl;
    movePlaneControl.name = "move_plane_control";
    movePlaneControl.always_visible = true;
    // movePlaneControl.orientation_mode = visualization_msgs::msg::InteractiveMarkerControl::FIXED;
    movePlaneControl.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::MOVE_PLANE;
    movePlaneControl.orientation.w = 1.0;
    movePlaneControl.orientation.x = 0.0;
    movePlaneControl.orientation.y = 1.0;
    movePlaneControl.orientation.z = 0.0;
    movePlaneControl.markers.push_back(marker);

    visualization_msgs::msg::InteractiveMarkerControl moveAxisControl;
    moveAxisControl.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::MOVE_AXIS;
    moveAxisControl.name = "move_axis_control";
    moveAxisControl.orientation.w = 1.0;
    moveAxisControl.orientation.x = 0.0;
    moveAxisControl.orientation.y = 1.0;
    moveAxisControl.orientation.z = 0.0;

    // 添加方向控制
    visualization_msgs::msg::InteractiveMarkerControl orientationControl;
    orientationControl.name = "orientation_control";
    orientationControl.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::ROTATE_AXIS;
    orientationControl.orientation.w = 1.0;
    orientationControl.orientation.x = 0.0;
    orientationControl.orientation.y = 1.0;
    orientationControl.orientation.z = 0.0;

    // 定义交互式Marker
    visualization_msgs::msg::InteractiveMarker intMarker;
    intMarker.header.frame_id = "map"; // 坐标系
    // intMarker.header.stamp = rosNode->get_clock()->now();
    intMarker.name = generateIntMarkerName("waypoint"); // 唯一名称
    intMarker.scale = 0.7;
    // 初始位置和姿态
    intMarker.pose.position.x = pose.position.x;
    intMarker.pose.position.y = pose.position.y;
    intMarker.pose.position.z = pose.position.z;

    intMarker.pose.orientation = pose.orientation;
    // intMarker.description = "Drag to move";   // 描述文本

    // push controls
    intMarker.controls.push_back(movePlaneControl);
    intMarker.controls.push_back(moveAxisControl);
    intMarker.controls.push_back(orientationControl);

    return intMarker;
}


InteractiveMarker InteractiveMarkerFactory::createFlyzonePoint(QColor color, geometry_msgs::msg::Point position)
{
    // 添加一个waypoint marker
    visualization_msgs::msg::Marker marker;
    marker.type = visualization_msgs::msg::Marker::SPHERE;
    marker.scale.x = 0.3; // 尺寸
    marker.scale.y = 0.3;
    marker.scale.z = 0.3;
    marker.color.r = color.redF(); // 颜色（RGBA）
    marker.color.g = color.greenF();
    marker.color.b = color.blueF();
    marker.color.a = color.alphaF();

    visualization_msgs::msg::InteractiveMarkerControl movePlaneControl;
    movePlaneControl.name = "move_plane_control";
    movePlaneControl.always_visible = true;
    // movePlaneControl.orientation_mode = visualization_msgs::msg::InteractiveMarkerControl::FIXED;
    movePlaneControl.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::MOVE_PLANE;
    movePlaneControl.orientation.w = 1.0;
    movePlaneControl.orientation.x = 0.0;
    movePlaneControl.orientation.y = 1.0;
    movePlaneControl.orientation.z = 0.0;
    movePlaneControl.markers.push_back(marker);

    visualization_msgs::msg::InteractiveMarkerControl moveAxisControl;
    moveAxisControl.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::MOVE_AXIS;
    moveAxisControl.name = "move_axis_control";
    moveAxisControl.orientation.w = 1.0;
    moveAxisControl.orientation.x = 0.0;
    moveAxisControl.orientation.y = 1.0;
    moveAxisControl.orientation.z = 0.0;
    

    // 定义交互式Marker
    visualization_msgs::msg::InteractiveMarker intMarker;
    intMarker.header.frame_id = "map"; // 坐标系
    // intMarker.header.stamp = rosNode->get_clock()->now();
    intMarker.name = generateIntMarkerName("flyzone_point"); // 唯一名称
    intMarker.scale = 0.7;
    // 初始位置和姿态
    intMarker.pose.position.x = position.x;
    intMarker.pose.position.y = position.y;
    intMarker.pose.position.z = position.z;
    intMarker.pose.orientation.x = 0.0;
    intMarker.pose.orientation.y = 0.0;
    intMarker.pose.orientation.z = 0.0;
    intMarker.pose.orientation.w = 1.0;
    // intMarker.description = "Drag to move";   // 描述文本

    // push controls
    intMarker.controls.push_back(movePlaneControl);
    intMarker.controls.push_back(moveAxisControl);
    return intMarker;
}


InteractiveMarker InteractiveMarkerFactory::createPcdPoint(PointNormal point)
{
    InteractiveMarker intMarker;
    intMarker.header.frame_id = "map";
    intMarker.name = generateIntMarkerName("pcd_point");
    intMarker.scale = 0.7f;
    // 初始位置和姿态
    intMarker.pose.position.x = point.position.x;
    intMarker.pose.position.y = point.position.y;
    intMarker.pose.position.z = point.position.z;
    // intMarker.description = "Non-interactive point";

    // 创建可视化部分
    visualization_msgs::msg::Marker pointMarker;
    pointMarker.type = visualization_msgs::msg::Marker::SPHERE;
    pointMarker.action = visualization_msgs::msg::Marker::ADD;
    pointMarker.scale.x = 0.3; // 尺寸
    pointMarker.scale.y = 0.3;
    pointMarker.scale.z = 0.3;
    pointMarker.color.r = 0.0f; // 颜色（RGBA）
    pointMarker.color.g = 0.0f;
    pointMarker.color.b = 1.0f;
    pointMarker.color.a = 0.5f;

    // 将多边形添加到 InteractiveMarker
    visualization_msgs::msg::InteractiveMarkerControl control;
    control.always_visible = true;
    control.markers.push_back(pointMarker);
    control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::NONE;
    intMarker.controls.push_back(control);

    json j = point;
    QString pointStr = QString::fromStdString(j.dump());
    addMetadata(intMarker, "point_normal", pointStr);

    return intMarker;
}


InteractiveMarker InteractiveMarkerFactory::createCube(std::string name, QColor markerColor, std::list<InteractiveMarker> &pointMarkers)
{
    auto firstPoint = pointMarkers.front();
    auto it = std::next(pointMarkers.begin(), 1);
    auto secondPoint = *it;

    geometry_msgs::msg::Point minPoint = firstPoint.pose.position;
    geometry_msgs::msg::Point maxPoint = secondPoint.pose.position;

    InteractiveMarker intMarker;
    intMarker.header.frame_id = "map";
    intMarker.name = name;

    // 位置（中心点）
    intMarker.pose.position.x = (maxPoint.x + minPoint.x)/2;
    intMarker.pose.position.y = (maxPoint.y + minPoint.y)/2;
    intMarker.pose.position.z = (maxPoint.z + minPoint.z)/2;

    // 计算盒子尺寸（局部坐标系长度宽度高度）
    float length = maxPoint.x - minPoint.x;
    float width = maxPoint.y - minPoint.y;
    float height = maxPoint.z - minPoint.z;

    // 创建一个cube marker
    visualization_msgs::msg::Marker cubeMarker;
    cubeMarker.type = visualization_msgs::msg::Marker::CUBE;
    cubeMarker.scale.x = length;
    cubeMarker.scale.y = width;
    cubeMarker.scale.z = height;

    cubeMarker.color.r = markerColor.redF(); // 颜色（RGBA）
    cubeMarker.color.g = markerColor.greenF();
    cubeMarker.color.b = markerColor.blueF();
    cubeMarker.color.a = markerColor.alphaF();

    // 创建控制器，将marker添加进去
    visualization_msgs::msg::InteractiveMarkerControl control;
    control.always_visible = true;
    control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::NONE;
    control.markers.push_back(cubeMarker);
    intMarker.controls.push_back(control);
    return intMarker;
}


InteractiveMarker InteractiveMarkerFactory::createPolygon(std::string name, QColor markerColor, std::list<InteractiveMarker> &pointMarkers)
{
    std::vector<geometry_msgs::msg::Point> points;
    if (pointMarkers.size() >= 3)
    {
        for (auto pointMarker : pointMarkers)
        {
            geometry_msgs::msg::Point point = pointMarker.pose.position;
            points.push_back(point);
        }
    }
    else
    {
        for (size_t i = 0; i <= 2; ++i)
        {
            geometry_msgs::msg::Point point;
            point.x = 0.0;
            point.y = 0.0;
            point.z = 0.0;
            points.push_back(point);
        }
    }

    InteractiveMarker intMarker;
    intMarker.header.frame_id = "map";
    intMarker.name = name;
    // intMarker.description = "Non-interactive polygon";

    // 创建多边形的可视化部分
    visualization_msgs::msg::Marker polygonMarker;
    polygonMarker.type = visualization_msgs::msg::Marker::TRIANGLE_LIST;
    polygonMarker.action = visualization_msgs::msg::Marker::ADD;
    polygonMarker.color.r = markerColor.redF(); // 颜色（RGBA）
    polygonMarker.color.g = markerColor.greenF();
    polygonMarker.color.b = markerColor.blueF();
    polygonMarker.color.a = markerColor.alphaF();

    // 添加顶点
    for (size_t i = 1; i < points.size() - 1; ++i)
    {
        polygonMarker.points.push_back(points[0]);
        polygonMarker.points.push_back(points[i]);
        polygonMarker.points.push_back(points[i + 1]);
    }

    // 将多边形添加到 InteractiveMarker
    visualization_msgs::msg::InteractiveMarkerControl control;
    control.always_visible = true;
    control.markers.push_back(polygonMarker);
    control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::NONE;
    intMarker.controls.push_back(control);
    return intMarker;
}


InteractiveMarker InteractiveMarkerFactory::createPrism(std::string name, std::list<InteractiveMarker> &pointMarkers)
{
    pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclPcd(new pcl::PointCloud<pcl::PointXYZINormal>());
    for (auto pointMarker : pointMarkers)
    {
        QString pointStr = getMetadataItem(pointMarker, "point_normal");
        PointNormal pointNormal = json::parse(pointStr.toStdString()).get<PointNormal>();
        auto pclPoint = PointNormal::toPclPoint(pointNormal);
        pclPcd->push_back(pclPoint);
    }
    pcl::MomentOfInertiaEstimation<pcl::PointXYZINormal> estimation;
    estimation.setInputCloud(pclPcd);
    estimation.compute();

    pcl::PointXYZINormal minPointOBB, maxPointOBB, centroid;
    Eigen::Matrix3f rotationMatrix;

    estimation.getOBB(minPointOBB, maxPointOBB, centroid, rotationMatrix);

    InteractiveMarker intMarker;
    intMarker.header.frame_id = "map";
    intMarker.name = name;

    // 位置（中心点）
    intMarker.pose.position.x = centroid.x;
    intMarker.pose.position.y = centroid.y;
    intMarker.pose.position.z = centroid.z;

    // 旋转（用矩阵转换成四元数）
    intMarker.pose.orientation = eigenMatrix3fToQuaternion(rotationMatrix);

    // 计算盒子尺寸（局部坐标系长度宽度高度）
    float length = maxPointOBB.x - minPointOBB.x;
    float width = maxPointOBB.y - minPointOBB.y;
    float height = maxPointOBB.z - minPointOBB.z;

    // 创建一个cube marker
    visualization_msgs::msg::Marker cubeMarker;
    cubeMarker.type = visualization_msgs::msg::Marker::CUBE;
    cubeMarker.scale.x = length;
    cubeMarker.scale.y = width;
    cubeMarker.scale.z = height;

    cubeMarker.color.r = 0.0f;
    cubeMarker.color.g = 1.0f;
    cubeMarker.color.b = 0.0f;
    cubeMarker.color.a = 0.5f;

    // 创建控制器，将marker添加进去
    visualization_msgs::msg::InteractiveMarkerControl control;
    control.always_visible = true;
    control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::NONE;
    control.markers.push_back(cubeMarker);
    intMarker.controls.push_back(control);
    return intMarker;

}

// InteractiveMarker InteractiveMarkerFactory::createPrism(std::string name, std::list<InteractiveMarker> &pointMarkers)
// {
//     std::vector<geometry_msgs::msg::Point> planePoints;

//     if (pointMarkers.size() >= 3)
//     {
//         pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclPcd(new pcl::PointCloud<pcl::PointXYZINormal>());
//         for (auto pointMarker : pointMarkers)
//         {
//             QString pclPointStr = getMetadataItem(pointMarker, "pcl_point");
//             auto pclPoint = strToPclPoint(pclPointStr);
//             pclPcd->push_back(pclPoint);
//         }

//         Eigen::Vector4f coefficients;
//         fitPlane(pclPcd, coefficients);
//         auto projectedPcd = projectPointsToPlane(pclPcd, coefficients);
//         for (auto pclPoint : *projectedPcd)
//         {
//             geometry_msgs::msg::Point point;
//             point.x = pclPoint.x;
//             point.y = pclPoint.y;
//             point.z = pclPoint.z;
//             planePoints.push_back(point);
//         }
//     }

//     if (planePoints.size() == 0)
//     {
//         for (size_t i = 0; i <= 2; ++i)
//         {
//             geometry_msgs::msg::Point point;
//             point.x = 0.0;
//             point.y = 0.0;
//             point.z = 0.0;
//             planePoints.push_back(point);
//         }
//     }

//     InteractiveMarker intMarker;
//     intMarker.header.frame_id = "map";
//     intMarker.name = name;
//     // intMarker.description = "Non-interactive polygon";
//     // 创建多边形的可视化部分
//     visualization_msgs::msg::Marker polygonMarker;
//     polygonMarker.type = visualization_msgs::msg::Marker::TRIANGLE_LIST;
//     polygonMarker.action = visualization_msgs::msg::Marker::ADD;
//     polygonMarker.color.r = 1.0; // 颜色（RGBA）
//     polygonMarker.color.g = 0.0;
//     polygonMarker.color.b = 0.0;
//     polygonMarker.color.a = 1.0;

//     // 添加顶点
//     for (size_t i = 1; i < planePoints.size() - 1; ++i)
//     {
//         polygonMarker.points.push_back(planePoints[0]);
//         polygonMarker.points.push_back(planePoints[i]);
//         polygonMarker.points.push_back(planePoints[i + 1]);
//     }

//     // 将多边形添加到 InteractiveMarker
//     visualization_msgs::msg::InteractiveMarkerControl control;
//     control.always_visible = true;
//     control.markers.push_back(polygonMarker);
//     control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::NONE;
//     intMarker.controls.push_back(control);

//     return intMarker;
// }

}

}
